#define S_W 640
#define S_H 480

#include <vector>
#include <ode/ode.h>

#include "GLSL.hh"

#include "World.hh"
#include "Renderable.hh"
#include "Touchable.hh"

#include <GL/gl.h>
#include <GL/glu.h>



using namespace std;

dWorldID World::sOdeWorld;
dSpaceID World::sSpace;


#define MAX_CONTACTS 64
#define step_time 20

dReal contactPoints[20][3];
int contactPointsCount = 0;
dJointGroupID contactGroup;
dWorldID fOdeWorld; 


void 
//World::
collisionHandler(void *dataUnused, dGeomID o1, dGeomID o2) {
	// Since here there is just one space, we don't check for geometry type:
	// they are not spaces
	dContactGeom contacts[MAX_CONTACTS];
	// Check for collisions
	int collisions = dCollide(o1, o2, MAX_CONTACTS, contacts, sizeof(dContactGeom));
	printf("%d collision points\n", collisions);
 
	// Save contact points
	for (int i = 0; i < collisions; ++i) {
		dGeomID g1 = contacts[i].g1,
				g2 = contacts[i].g2;
 
		if (g1 == g2)
			continue;
 
		double *pos = contacts[i].pos;
		printf("Copoint %d: %f %f %f\n", i, pos[0], pos[1], pos[2]);
 
		contactPoints[contactPointsCount][0] = pos[0];
		contactPoints[contactPointsCount][1] = pos[1];
		contactPoints[contactPointsCount][2] = pos[2];
		contactPointsCount++;
 
		char *o1Name = (char*) dGeomGetData(g1);
		char *o2Name = (char*) dGeomGetData(g2);
		//		const dReal *o1Pos = dGeomGetPosition(g1);
		//              const dReal *o2Pos = dGeomGetPosition(g2);
 
		printf("Collision between %s (%x) and %s (%x)\n", o1Name, (unsigned int) g1, o2Name, (unsigned int) g2);
	}
}

dWorldID 
World::GetWorld(){
  return sOdeWorld;
}


static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

  dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
  for (i=0; i<MAX_CONTACTS; i++) {
    contact[i].surface.mode = dContactBounce;//| dContactSoftCFM;
    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.5;
    contact[i].surface.bounce_vel = 0.1;
    contact[i].surface.soft_cfm = 0.01;
  }
  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
                           sizeof(dContact))) {
    dMatrix3 RI;
    dRSetIdentity (RI);
    const dReal ss[3] = {0.02,0.02,0.02};
    for (i=0; i<numc; i++) {
      dJointID c = dJointCreateContact (fOdeWorld,contactGroup,contact+i);
      dJointAttach (c,b1,b2);
    }
  }
}

World::World(){
  cerr << "World() arg-less constructor: This should never be used" << endl;
  cout << "Exiting(-1)" << endl;
  exit(-1);
}

World::World(SDL_Surface *screen){
  dInitODE2(0);
  sOdeWorld = dWorldCreate();
  mContactgroup = dJointGroupCreate(0);
  mScreen = screen;
  mSelectedCamera = 0;
  //SetGravity(0, -4, 0); //-9.81,0);
    SetGravity(0, -9.81,0);
  //  dWorldSetERP(sOdeWorld, 0.8);
  dWorldSetCFM (sOdeWorld,1e-5);
  sSpace = dSimpleSpaceCreate(0);
  
}

World::~World(){
  cout << "Bye Bye!" << endl;
  dSpaceDestroy(sSpace);
  dWorldDestroy(sOdeWorld);
  dCloseODE();
}

void
World::SetGravity(dReal x, dReal y, dReal z){
  dWorldSetGravity(sOdeWorld, x, y, z);
}

void
World::SetCFM(dReal cfm){
  dWorldSetCFM(sOdeWorld, cfm);
}

void
World::AddEntity(Renderable* r){
  mRenderableList.push_back(r);
}

void
World::AddEntity(Touchable* t){
  t->on_AddWorld(sOdeWorld, sSpace);  
  mTouchableList.push_back(t);
}


void 
World::AddCamera(Camera* c){
  mCameraList.push_back(c);
}
  
void 
World::SelectCamera(unsigned int i){
  mSelectedCamera = i;
}

void
World::OdeSim(){
  //
}


void
World::Render(){    

  Touchable* t;
  Renderable* r;

  // glEnableClientState(GL_VERTEX_ARRAY);

  mItR = mRenderableList.begin();
  while( mItR != mRenderableList.end() ) {
    r = *mItR;
    r->Render();
    mItR++;
  }

  mItT = mTouchableList.begin();
  while( mItT != mTouchableList.end() ) {
    t = *mItT;
    t->Render();
    mItT++;
  }
  
  //  glDisableClientState(GL_VERTEX_ARRAY);

}


void
World::PrintAllNames(){
  

  Touchable* t;
  Renderable* r;

  cout << "Renderables:" << endl;
  mItR = mRenderableList.begin();
  while( mItR != mRenderableList.end() ) {
    r = *mItR;

    cout << "Name:\t" << r->GetName();
    cout << "\tPos: " << r->GetPosX();
    cout << "," << r->GetPosY();
    cout << "," << r->GetPosZ();
    cout << endl;

    mItR++;
  }

  cout << "Touchables:" << endl;
  mItT = mTouchableList.begin();
  while( mItT != mTouchableList.end() ) {
    t = *mItT;

    cout << "Name: " << t->GetName();
    cout << "\tPos: (" << t->GetPosX();
    cout << "," << t->GetPosY();
    cout << "," << t->GetPosZ() << ")";
    cout << "\tMass: " << t->GetMass() << endl;    

    mItT++;
  }
  
}

int
World::MainLoop(){
  // Here is where all the action is triggered  



  /*  unsigned char* RGB;
  unsigned char* RGB2;
  GLfloat* Z1;
  GLfloat* Z2;


  RGB = (unsigned char *) malloc(S_W * S_H * sizeof(unsigned char) * 3);
  RGB2 = (unsigned char *) malloc(S_W * S_H * sizeof(unsigned char) * 3);
  Z1 = (GLfloat*) malloc(S_W * S_H * sizeof(GLfloat));
  Z2 = (GLfloat*) malloc(S_W * S_H * sizeof(GLfloat));
  
  GLuint RGBtexture;  
  GLuint Dtexture;  
  */


  //OJO
  fOdeWorld = sOdeWorld;
  contactGroup = dJointGroupCreate(0);
  int pause= 1;


  // Test plane for ODE...
  dGeomID plane;
  dTriMeshDataID triMesh;
 // Plane geometry
  const int indexes[6] = {2, 1, 0, 3, 2, 0};

  const dVector3 triVert[4] = {
    { 100.0,  0.0,  100.0},
    {-100.0,  0.0,  100.0},
    {-100.0,  0.0, -100.0},
    { 100.0,  0.0, -100.0}
  };
  
  //plane = dCreateBox (sSpace,300,300,300); 
  //dGeomSetPosition(plane, 0, -155.0 , 0);

  triMesh = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSimple(triMesh, (dReal*)triVert, 4, (const dTriIndex*) indexes, 6);
  plane = dCreateTriMesh(sSpace, triMesh, NULL, NULL, NULL);
  dGeomSetData(plane, (void*) "Plane");
  dGeomSetPosition(plane, 0, -0.2, 0);
  

  

  int ticks;
  int elapsed_time = SDL_GetTicks();
  int last_step = elapsed_time;
  //  const int step_time = 1000;
  int fps = 0;

  bool exit_event = false;
  SDL_Event event;
 
  static const GLfloat lightPos[] = {1.f, 0.f, 0.f, 2.f};
  static const GLfloat AmbientColor[] = {1.f, 1.f, 1.f, 1.f};
  static const GLfloat DiffuseColor[] = {1.f, 1.f, 1.00, 1.f};
  static const GLfloat lmodel_ambient[] = {0.f, 0.f, 0.f, 1.f};

  PrintAllNames();
  

  glEnable(GL_NORMALIZE);
  glEnableClientState(GL_VERTEX_ARRAY);
  
  glLightfv(GL_LIGHT1, GL_AMBIENT, AmbientColor);
  glLightfv(GL_LIGHT1, GL_DIFFUSE, DiffuseColor);
  glLightModelfv(GL_LIGHT_MODEL_AMBIENT, lmodel_ambient);
  glDisable(GL_LIGHT0);
  glEnable(GL_LIGHT1);

  /* Enable lighting */
  glEnable(GL_LIGHTING);


  Uint32 last_time = SDL_GetTicks();
  Uint32 current_time,ellapsed_time;
  Uint32 start_time;
  
  while (!exit_event){
    start_time = SDL_GetTicks();

    contactPointsCount = 0;
    //    dSpaceCollide(sSpace, 0, collisionHandler);
    dSpaceCollide(sSpace, 0,  nearCallback);
    
    // Physics
    if (!(pause)) dWorldStepFast1 (sOdeWorld,0.05, 5);
    dJointGroupEmpty(contactGroup);


  
    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity();

    if ( (mCameraList.size() > mSelectedCamera) ){
      mCameraList.at(mSelectedCamera)->Use();
    }

 

    glLightfv(GL_LIGHT1, GL_POSITION, lightPos);

    // Render Renderables and Touchables (I hope to Render a Map soon too)
            dVector3 v1, v2, v3;
    const dReal *realP;
        glPushMatrix();
                glColor3f(1.0, 0.3, 0.3);
		//realP = dGeomGetPosition(plane);
		//glTranslatef(realP[0], realP[1], realP[2]);
                glBegin(GL_TRIANGLES);
                        for (int i = 0; i < 2; ++i) {
			  dGeomTriMeshGetTriangle(plane, i, (dReal(*)[4]) v1, (dReal(*)[4]) v2, (dReal(*)[4]) v3);
                        //      printf("Triangle %d: %2.1f %2.1f %2.1f, %2.1f %2.1f %2.1f, %2.1f %2.1f %2.1f\n", i,
                        //              v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], v3[0], v3[1], v3[2]);
			   glVertex3dv((const GLdouble*) v1);
			   glVertex3dv((const GLdouble*) v2);
			   glVertex3dv((const GLdouble*) v3);
                        }
                glEnd();
		glPopMatrix();
    
	this->Render();

     
        
    SDL_GL_SwapBuffers();
    

    


    /*/ FPS
    ticks = SDL_GetTicks();
    ++fps;
    if ((ticks - last_step) > step_time) {
      last_step = ticks;
      printf("fps = %d\n", fps);
      fps = 0;      
      }
    elapsed_time = ticks;
    */


    // Events
    while ( SDL_PollEvent(&event) )
      {
	if ( event.type == SDL_QUIT )  {   exit_event = true;  }
	
	if ( event.type == SDL_KEYDOWN ){
	  if ( event.key.keysym.sym == SDLK_ESCAPE ) { exit_event = true; }
	  if ( event.key.keysym.sym == SDLK_SPACE ) { 
	    AddEntity(new Touchable(string("brue"), float((rand()-RAND_MAX/2)/float(RAND_MAX))*30, float((rand())/float(RAND_MAX))*30+30, float((rand())/float(RAND_MAX))*(-30)-20 , "modelos/1.md3", 10, 0,-10,0));
	    std::cout << "#: " << mTouchableList.size() << std::endl;
	  }
	  if ( event.key.keysym.sym == SDLK_UP ) {
	     Touchable* t;
  
	     mItT = mTouchableList.begin();
	     while( mItT != mTouchableList.end() ) {
	       t = *mItT;
	       
	       t->SetVel(0,t->mVely+(rand())/float(RAND_MAX)*10,0);
	       
	       mItT++;
	     }
	  }

	  if ( event.key.keysym.sym == SDLK_DOWN ) {
	    /*Touchable* t;
  
	     mItT = mTouchableList.begin();
	     while( mItT != mTouchableList.end() ) {
	       t = *mItT;
	       
	       t->SetVel(0,t->mVely-(rand())/float(RAND_MAX)*10,0);
	       
	       mItT++;
	       }*/
	    pause = !pause;
	  }

	}
	int x,y;
	if( event.type == SDL_MOUSEMOTION ) { 
	  SDL_GetRelativeMouseState(&x, &y);
	  if ( (mCameraList.size() > mSelectedCamera) ){
	    mCameraList.at(mSelectedCamera)->mX+=x;
	    mCameraList.at(mSelectedCamera)->mY+=y;
	  }
	}
      } 
    
    //    SDL_Delay(20);
    
    ellapsed_time = SDL_GetTicks() - start_time;
    if (ellapsed_time < 10)
      SDL_Delay(10 - ellapsed_time);

    
  } // while !exit_event
  
  return 0;
}

